/*
 * @Description: 
 * @Author: Sang Hao
 * @Date: 2021-11-16 02:55:23
 * @LastEditTime: 2021-11-22 21:35:27
 * @LastEditors: Sang Hao
 */

#include <pcl/PointIndices.h>
#include <pcl/filters/extract_indices.h>
#include "lidar_slam/models/cloud_filter/range_filter.hpp"

namespace lidar_slam {
RangeFilter::RangeFilter(const YAML::Node& config_node) {
	range_min_ = config_node["range_min"].as<float>();
	range_max_ = config_node["range_max"].as<float>();
}

RangeFilter::RangeFilter(float range_min, float range_max) 
	: range_min_(range_min), range_max_(range_max) {
	
}

bool RangeFilter::Filter(const CloudData::CLOUD_PTR& input_cloud_ptr, 
		CloudData::CLOUD_PTR& filtered_cloud_ptr) {
	pcl::PointIndices::Ptr outlier_indices(new pcl::PointIndices);
	for (size_t i = 0; i < input_cloud_ptr->points.size(); ++i) {
		float x = input_cloud_ptr->points[i].x;
		float y = input_cloud_ptr->points[i].y;
		if ((x * x + y * y > range_max_ * range_max_) || 
			(x * x + y * y < range_min_ * range_min_)) {
				outlier_indices->indices.push_back(i);
		}
	}
	
	pcl::ExtractIndices<CloudData::POINT> extract;

	extract.setInputCloud(input_cloud_ptr);
	extract.setIndices(outlier_indices);	
	extract.setNegative(true);	// true表示indices中的点都是不要的，因此为setNegative
	extract.filter(*filtered_cloud_ptr);

	return true;
}
}